/*
 ============================================================================
 Name        : TestProj.c
 Author      : 
 Version     :
 Copyright   : Your copyright notice
 Description : Hello World in C, Ansi-style
 ============================================================================
 */

#include <stdlib.h>
#include <fcntl.h>
#include <math.h>
#include "termios.h"
#include "protocol.h"
#include "string.h"
#include "video.h"

int open_serial(char* line);
void set_position(SerialConn* conn, char motor, double newPos);
void get_position(SerialConn* conn);
void reset(SerialConn* conn);

int main()
{
   int fd;
   SerialConn conn;
   conn.state = START;
   char* line = "/dev/ttyO0";
   fd = open_serial(line);
   conn.fd = fd;
   unsigned char rxBuffer[1];
   int ret;
   unsigned char packet[10];
   packet[0] = 0x23;
   packet[1] = 0xd9;
   packet[2] = 0xce;
   packet[3] = 0xf7;
   packet[4] = 0xd9;
   packet[5] = 0xe3;
   packet[6] = 0xa5;
   packet[7] = 0x03;
   packet[8] = 0x40;

   if (fd)
   {
      //get_position(&conn);
      //reset(&conn);

      start_video();

      double step = 0.01;
      double i = 0;
      double last_out = 0;
      while (1)
      {
         double out = 0.5 * sin(i);
         set_position(&conn, SET_HORIZONTAL, out - last_out);
         set_position(&conn, SET_VERTICAL, out - last_out);
         i += step;
         last_out = out;
         usleep(10000);
      }
      set_position(&conn, SET_HORIZONTAL, 0);
      set_position(&conn, SET_VERTICAL, 0);
      //write_packet(fd, packet, 9);
      //write(fd, &packet, 1);
      while (1)
      {
         //write_packet(fd, packet, 6);
         //printf("%c\n", txBuffer[0]);
         //write(fd, &txBuffer, 1);
         ret = read(fd, &rxBuffer, 1);
         if (ret != -1)
            printf("%x\n", rxBuffer[0]);
         if (ret != -1 && parse_next(&conn, rxBuffer[0]))
         {
            printf("packet complete\n");
            //packet complete
         }
      }
      printf("jojoer\n");
   }

   close(fd);
}

int open_serial(char* line)
{
   int fd;
   struct termios tty_attributes;

   if ((fd = open(line, O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0)
   {
      fprintf(stderr, "Open error on \n");
      return 0;
   }
   else
   {
      tcgetattr(fd, &tty_attributes);
      cfmakeraw(&tty_attributes);
      tcsetattr(fd, TCSANOW, &tty_attributes);

      return fd;
   }
}

void set_position(SerialConn* conn, char motor, double newPos)
{
   unsigned char pkt[9];
   pkt[0] = motor;
   double *p = (double*)(pkt + 1);
   *p = newPos;
   int i;
   //for (i=0;i<8;i++)
   //{
   //   printf("a: %x\n", pkt[i+1]);
   //}
   double* t;
   t = (double*)(pkt+1);
   //printf("d: %f", *t);
   write_packet(conn->fd, pkt, 9);
   //printf("jojo\n");
}

void get_position(SerialConn* conn)
{
   unsigned char pkt[1];
   pkt[0] = GET_POSITION;
   write_packet(conn->fd, pkt, 1);
}

void reset(SerialConn* conn)
{
   unsigned char pkt[1];
   pkt[0] = RESET;
   write_packet(conn->fd, pkt, 1);
}
